{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Jupyter environment detected. Enabling Open3D WebVisualizer.\n",
      "[Open3D INFO] WebRTC GUI backend enabled.\n",
      "[Open3D INFO] WebRTCWindowSystem: HTTP handshake server disabled.\n"
     ]
    }
   ],
   "source": [
    "import open3d as o3d\n",
    "import copy\n",
    "import numpy as np"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This tutorial demonstrates an ICP variant that uses both geometry and color for registration. It implements the algorithm of [Park2017]. The color information locks the alignment along the tangent plane. Thus this algorithm is more accurate and more robust than prior point cloud registration algorithms, while the running speed is comparable to that of ICP registration. This tutorial uses notations from ICP registration.\n",
    "## Helper visualization function\n",
    "In order to demonstrate the alignment between colored point clouds, draw_registration_result_original_color renders point clouds with their original color."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "def draw_registration_result_original_color(source, target, transformation):\n",
    "    source_temp = copy.deepcopy(source)\n",
    "    source_temp.transform(transformation)\n",
    "    o3d.visualization.draw_geometries([source_temp, target],\n",
    "                                      zoom=0.5,\n",
    "                                      front=[-0.2458, -0.8088, 0.5342],\n",
    "                                      lookat=[1.7745, 2.2305, 0.9787],\n",
    "                                      up=[0.3109, -0.5878, -0.7468])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Input\n",
    "The code below reads a source point cloud and a target point cloud from two files. An identity matrix is used as initialization for the registration."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "1. Load two point clouds and show initial pose\n",
      "[Open3D INFO] Downloading https://github.com/isl-org/open3d_downloads/releases/download/20220201-data/DemoColoredICPPointClouds.zip\n",
      "\u001b[1;33m[Open3D WARNING] Failed to download from https://github.com/isl-org/open3d_downloads/releases/download/20220201-data/DemoColoredICPPointClouds.zip. Exception \u001b[1;31m[Open3D Error] (std::string open3d::utility::DownloadFromURL(const string&, const string&, const string&)) /root/Open3D/cpp/open3d/utility/Download.cpp:153: Download failed with error code: Timeout was reached.\n",
      "\u001b[0;m.\u001b[0;m\n"
     ]
    },
    {
     "ename": "RuntimeError",
     "evalue": "\u001b[1;31m[Open3D Error] (std::string open3d::utility::DownloadFromMirrors(const std::vector<std::basic_string<char> >&, const string&, const string&)) /root/Open3D/cpp/open3d/utility/Download.cpp:184: Downloading failed from available mirrors.\n\u001b[0;m",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mRuntimeError\u001b[0m                              Traceback (most recent call last)",
      "Cell \u001b[0;32mIn[4], line 2\u001b[0m\n\u001b[1;32m      1\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124m1. Load two point clouds and show initial pose\u001b[39m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m----> 2\u001b[0m demo_colored_icp_pcds \u001b[38;5;241m=\u001b[39m o3d\u001b[38;5;241m.\u001b[39mdata\u001b[38;5;241m.\u001b[39mDemoColoredICPPointClouds()\n\u001b[1;32m      3\u001b[0m source \u001b[38;5;241m=\u001b[39m o3d\u001b[38;5;241m.\u001b[39mio\u001b[38;5;241m.\u001b[39mread_point_cloud(demo_colored_icp_pcds\u001b[38;5;241m.\u001b[39mpaths[\u001b[38;5;241m0\u001b[39m])\n\u001b[1;32m      4\u001b[0m target \u001b[38;5;241m=\u001b[39m o3d\u001b[38;5;241m.\u001b[39mio\u001b[38;5;241m.\u001b[39mread_point_cloud(demo_colored_icp_pcds\u001b[38;5;241m.\u001b[39mpaths[\u001b[38;5;241m1\u001b[39m])\n",
      "\u001b[0;31mRuntimeError\u001b[0m: \u001b[1;31m[Open3D Error] (std::string open3d::utility::DownloadFromMirrors(const std::vector<std::basic_string<char> >&, const string&, const string&)) /root/Open3D/cpp/open3d/utility/Download.cpp:184: Downloading failed from available mirrors.\n\u001b[0;m"
     ]
    }
   ],
   "source": [
    "print(\"1. Load two point clouds and show initial pose\")\n",
    "demo_colored_icp_pcds = o3d.data.DemoColoredICPPointClouds()\n",
    "source = o3d.io.read_point_cloud(demo_colored_icp_pcds.paths[0])\n",
    "target = o3d.io.read_point_cloud(demo_colored_icp_pcds.paths[1])\n",
    "\n",
    "# draw initial alignment\n",
    "current_transformation = np.identity(4)\n",
    "draw_registration_result_original_color(source, target, current_transformation)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Point-to-plane ICP\n",
    "We first run Point-to-plane ICP as a baseline approach. The visualization below shows misaligned green triangle textures. This is because a geometric constraint does not prevent two planar surfaces from slipping."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# point to plane ICP\n",
    "current_transformation = np.identity(4)\n",
    "print(\"2. Point-to-plane ICP registration is applied on original point\")\n",
    "print(\"   clouds to refine the alignment. Distance threshold 0.02.\")\n",
    "result_icp = o3d.pipelines.registration.registration_icp(\n",
    "    source, target, 0.02, current_transformation,\n",
    "    o3d.pipelines.registration.TransformationEstimationPointToPlane())\n",
    "print(result_icp)\n",
    "draw_registration_result_original_color(source, target,\n",
    "                                        result_icp.transformation)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Colored point cloud registration"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# colored pointcloud registration\n",
    "# This is implementation of following paper\n",
    "# J. Park, Q.-Y. Zhou, V. Koltun,\n",
    "# Colored Point Cloud Registration Revisited, ICCV 2017\n",
    "voxel_radius = [0.04, 0.02, 0.01]\n",
    "max_iter = [50, 30, 14]\n",
    "current_transformation = np.identity(4)\n",
    "print(\"3. Colored point cloud registration\")\n",
    "for scale in range(3):\n",
    "    iter = max_iter[scale]\n",
    "    radius = voxel_radius[scale]\n",
    "    print([iter, radius, scale])\n",
    "\n",
    "    print(\"3-1. Downsample with a voxel size %.2f\" % radius)\n",
    "    source_down = source.voxel_down_sample(radius)\n",
    "    target_down = target.voxel_down_sample(radius)\n",
    "\n",
    "    print(\"3-2. Estimate normal.\")\n",
    "    source_down.estimate_normals(\n",
    "        o3d.geometry.KDTreeSearchParamHybrid(radius=radius * 2, max_nn=30))\n",
    "    target_down.estimate_normals(\n",
    "        o3d.geometry.KDTreeSearchParamHybrid(radius=radius * 2, max_nn=30))\n",
    "\n",
    "    print(\"3-3. Applying colored point cloud registration\")\n",
    "    result_icp = o3d.pipelines.registration.registration_colored_icp(\n",
    "        source_down, target_down, radius, current_transformation,\n",
    "        o3d.pipelines.registration.TransformationEstimationForColoredICP(),\n",
    "        o3d.pipelines.registration.ICPConvergenceCriteria(relative_fitness=1e-6,\n",
    "                                                          relative_rmse=1e-6,\n",
    "                                                          max_iteration=iter))\n",
    "    current_transformation = result_icp.transformation\n",
    "    print(result_icp)\n",
    "draw_registration_result_original_color(source, target,\n",
    "                                        result_icp.transformation)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "notes",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.11.7"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
